#include "Magnet.h"
#include "GlutStuff.h"
#include "btBulletDynamicsCommon.h"
#include "GLDebugDrawer.h"

#include "ParticleSystem.h"
#include <iostream>

list<Magnet*> Magnet::MagnetList;
btDynamicsWorld* Magnet::m_dynamicsWorld;
btAlignedObjectArray<btCollisionShape*>	Magnet::m_collisionShapes;

const float Magnet::k = 10000.0f;
const float Magnet::inverseU0 = 1.0f / (Magnet::k * 4 * PI);
int Magnet::numMagnet = 0;


Vector3 mFieldSystem[mFieldSystemSize][mFieldSystemSize];


void Magnet::Simulation() {

	BT_PROFILE("MagnetSimulation");

	if (MagnetList.empty()) return;

	for (auto it = MagnetList.begin(); it != MagnetList.end(); it++) {

		Magnet* mg = (*it);
		if ( mg->isFerroMagnet ) {
			mg->_ComputeInducedMagnetization();
		}
		else {
			mg->_UpdateCellMagneticMoment();
		}

		mg->_UpdateCellPosition();
	}

	for (auto it1 = MagnetList.begin(); it1 != --MagnetList.end(); it1++) {
		Magnet *mi = (*it1);
		++it1;
		for (auto it2 = it1; it2 != MagnetList.end(); it2++) {
			Magnet *mk = (*it2);

			mi->_ComputeForceAndTorque(mk);
			mk->_ComputeForceAndTorque(mi);
		}
		--it1;
	}
	

}


void Magnet::AddMagnet(Magnet* mg) {

	MagnetList.push_back(mg);
	numMagnet++;
}

void Magnet::SetDynamicsWorld(btDynamicsWorld* dw, btAlignedObjectArray<btCollisionShape*> cs) {
	MagnetList.clear();
	m_dynamicsWorld = dw;
	m_collisionShapes = cs;
}

btVector3 Magnet::CalcMField(Magnet* mg, const btVector3& pos) {
	BT_PROFILE("CalcMField");
	btVector3 MField(0., 0., 0.);

	for (auto it = MagnetList.begin(); it != MagnetList.end(); it++) {

		if (mg && (*it)->Equal(mg))
			continue;
		
		MField += (*it)->_ComputeMField(pos);

	}

#ifdef _DEBUG
//	printf("MField : %f %f %f\n", MField.x(), MField.y(), MField.z());
#endif

	return MField;
}

void Magnet::RegisterToWorld() {

	transform.setIdentity();
	transform.setOrigin(position);
	transform.setRotation(orientation);

	btBoxShape* colShape;

	colShape = new btBoxShape(scale);
	bool isDynamic = (mass != 0.f);

	btVector3 localInertia(0,0,0);
	if (isDynamic)
		colShape->calculateLocalInertia(mass,localInertia);

	m_collisionShapes.push_back(colShape);

	btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);

	if (body) 
		m_dynamicsWorld->removeRigidBody(body);

	body = new btRigidBody(rbInfo);
	body->setFriction(10);
	body->setRestitution(0.2);
	body->setDamping(0.1, 0.1);
	m_dynamicsWorld->addRigidBody(body);

	_SetCellLocalPosition();
	_UpdateCellPosition();
	if (!isFerroMagnet)
		_UpdateCellMagneticMoment();
	/*
	for(int x=0; x<mFieldSystemSize; x++){
		for(int z=0; z<mFieldSystemSize;z++){
			mFieldSystem[x][z] = CalcMField(NULL,btVector3((float)x/mFieldSystemSize, 0.5, (float)z/mFieldSystemSize));
		}
	}*/

}


void Magnet::_SetCellLocalPosition() {

	float **pos = new float*[numCells];
	for (int i=0; i<numCells; i++)
		pos[i] = new float[3];
	
	btVector3 curScale = scale / 2;
	pos[0][0] = curScale.x();
	pos[0][1] = curScale.y();
	pos[0][2] = curScale.z();

	int n = numCells;;

	for (int i=1; i<n; i*=2) {

		int sel;
		float sc;
		if ( curScale.x() > curScale.y() && curScale.x() > curScale.z() ) {
			// x is max
			curScale.setX( sc = (curScale.getX() / 2.0f) );
			sel = 0;
		}
		else if ( curScale.y() > curScale.z() ) {
			// y is max
			curScale.setY( sc = (curScale.getY() / 2.0f) );
			sel = 1;
		}
		else {
			// z is max
			curScale.setZ( sc = (curScale.getZ() / 2.0f) );
			sel = 2;
		}

		for (int j=i-1; j>=0; j--) {
			for (int k=1; k>=0; k--) {
				for (int l=0; l<3; l++) {
					pos[2*j+k][l] = pos[j][l];
				}
				pos[2*j+k][sel] = pos[j][sel] + (k*2-1)*sc;
			}
		}
	}

	btVector3 moveCenter = scale / 2.0f;

	for (int i=0; i<n; i++) {
		btVector3 locpos = btVector3(pos[i][0], pos[i][1], pos[i][2]);
		locpos -= moveCenter;
		cells[i].setLocalPosition(locpos*2);
	//	PrintVector3(locpos);printf("\n");
	}
}

void Magnet::_UpdateCellPosition() {
	if (!body) return;
	btTransform transform = body->getWorldTransform();

	for (int i=0; i<numCells; i++) 
		cells[i].updateWorldPosition(transform);
	
}

void Magnet::_UpdateCellMagneticMoment() {
	auto q = body->getWorldTransform().getRotation();
	btVector3 axis = q.getAxis();
	btScalar angle = q.getAngle();
	btVector3 mm = magnetization.rotate(axis, angle);
	mm *= (volume * invNumCells);

	for (int i=0; i<numCells; i++) {
		
		cells[i].setMagneticMoment( mm );
	}
}

btVector3 Magnet::_ComputeMField(const btVector3& pos) {
	BT_PROFILE("_ComputeMField");
	btVector3 MField(0., 0., 0.);

	for (int i=0; i<numCells; i++) {

		btVector3 dir = pos - cells[i].getPosition();
		if (dir.length() > 10000.0f) continue;	// ignore if distance is too long
		btVector3 n = dir.normalized();
		btVector3 mi = cells[i].getMoment();

		btVector3 field = 3 * n * n.dot(mi);
		field -= mi;

		// smoothing for close distance
		btScalar dis = dir.length();
		dis += 0.01f;

		field *= pow(dis, -3);

		MField += field;
	}

	return MField * k;
}

void Magnet::_ComputeInducedMagnetization() {
	BT_PROFILE("_ComputeInducedMagnetization");
	for (int i=0; i<numCells; i++) {
		btVector3 mf = CalcMField(this, cells[i].getPosition());

		cells[i].setMagneticMoment(mf * calcScaleInduce );
	}
}

void Magnet::_ComputeForceAndTorque(Magnet *mgl) {
	BT_PROFILE("_ComputeForceAndTorque");
#ifdef _DEBUG
	btVector3 tot_mfc(0.,0.,0.), tot_mtq(0.,0.,0.);
#endif
	if (driver) return;

	for (int i=0; i<numCells; i++) {
		Cell ck = cells[i];
		for (int j=0; j<mgl->numCells; j++) {
			
			Cell cl = mgl->cells[j];
			
			btVector3 dir = ck.getPosition() - cl.getPosition();
			btVector3 n = dir.normalized();

			btVector3 mk = ck.getMoment();
			btVector3 mi = cl.getMoment();

			btVector3 mfc = -5 * n * mk.dot(n) * mi.dot(n);
			mfc += n * mk.dot(mi);
			mfc += mk * mi.dot(n) + mi * mk.dot(n);
			mfc *= 3;

			btVector3 mtq = 3 * mk.cross(n) * mi.dot(n);
			mtq -= mk.cross(mi);

			// smoothing for close distance
			btScalar dis = dir.length();
			//dis *= 1.3f;
			dis += 0.02f;

			mfc *= pow(dis, -4);
			mtq *= pow(dis, -3);
#ifdef _DEBUG
			tot_mfc += mfc;
			tot_mtq += mtq;
			//printf("Force : %f %f %f. Torque : %f %f %f\n",mfc.x(), mfc.y(), mfc.z(), mtq.x(), mtq.y(), mtq.z());
#endif
			body->applyCentralForce(mfc * k);
			//body->applyForce(mfc * k, ck.getPosition());
			body->applyTorque(mtq * k);
			
		}
	}
#ifdef _DEBUG
	//printf("Force : %f %f %f. Torque : %f %f %f\n",tot_mfc.x(), tot_mfc.y(), tot_mfc.z(), tot_mtq.x(), tot_mtq.y(), tot_mtq.z());
#endif
	body->activate();
}

const Vector3 Magnet::GetMagnetForce(const Vector3& pk, const Vector3& mk) {

	Vector3 force;

	if (MagnetList.empty()) return force;

	auto end = MagnetList.end();
	for (auto it = MagnetList.begin(); it != end; it++) {
		Magnet* m = (*it);
		for (int i=0; i < m->numCells; i++) {
			Cell ci = m->cells[i];
			Vector3 mi = ci.getMoment();
			Vector3 pi = ci.getPosition();

			Vector3 dir = pk - pi;
			Vector3 n = dir.normalized();

			Vector3 mfc = -5 * n * mk.dot(n) * mi.dot(n);
			mfc += n * mk.dot(mi);
			mfc += mk * mi.dot(n) + mi * mk.dot(n);
			mfc *= 3;

			float dis = dir.length_square();
			mfc /= dis * dis;

			force += mfc;
		}
	}

	return force * k;
}

Vector3 Magnet::_ComputeMFieldSystem(Vector3& pos){
	if(pos.x()>=1){pos = Vector3(0.9999,0.5,pos.z());}
	if(pos.z()>=1){pos = Vector3(pos.x(),0.5,0.9999);}
	if(pos.x()<0){pos = Vector3(0,0.5,pos.z());}
	if(pos.z()<0){pos = Vector3(pos.x(),0.5,0);}

	cout << (int)floor(pos.x()*mFieldSystemSize) << "\t" <<(int)floor(pos.z()*mFieldSystemSize) << endl;

	
	return mFieldSystem[(int)floor(pos.x()*mFieldSystemSize)][(int)floor(pos.z()*mFieldSystemSize)];
}

void PrintVector3(const btVector3& v) {
	printf("(%f %f %f)",v.x(), v.y(), v.z());
}


const Vector3 Magnet::ComputeMagnetForce(const Vector3& pos, const Vector3& other_pos, const Vector3& moment, const Vector3& other_moment) {
	
	return 0.f;
}

const Vector3 Magnet::ComputeMagnetField(const Vector3& pos, const Vector3& magnet_pos, const Vector3& moment) {

	btVector3 dir = pos - magnet_pos;
	
	btVector3 n = dir.normalized();
	btVector3 mi = moment;

	btVector3 field = 3 * n * n.dot(mi) - mi;
	
	btScalar dis = dir.length();
	field *= pow(dis, -3) * k;

	return field;

}